#ifndef ROBOT_PAYLOAD_H
#define ROBOT_PAYLOAD_H

/*
 * Payload class
 * Team 675 - TJ Dooley
 * Created February 2012
 *
 * Handles Payload Systems 
 */

#include "Include.h"
#include "BeltSystem.h"
#include "Turret.h"

namespace Team675
{

class RobotPayload
{
public:
	typedef enum {CONTROL_AUTONOMOUS, CONTROL_KINECT, CONTROL_SINGLE_JOYSTICK, CONTROL_DOUBLE_JOYSTICK, CONTROL_DISABLED} ControlMode;
	typedef enum {TRACKING_AUTONOMOUS, TRACKING_AUTOMATIC_CAMERA, TRACKING_MANUAL_PID, TRACKING_MANUAL, TRACKING_DISABLED} TrackingMode;
	typedef enum {SHOOTING_AUTOMATIC, SHOOTING_CONFIDENCE, SHOOTING_MANUAL, SHOOTING_DISABLED} ShootingMode;
	
	RobotPayload(ControlMode _controlMode = CONTROL_DISABLED, 
			TrackingMode _trackingMode = TRACKING_DISABLED, 
			ShootingMode _shootingMode = SHOOTING_DISABLED);
	
	void Update();
	
	void SetControlMode(ControlMode _mode);
	void SetTrackingMode(TrackingMode _mode);
	void SetShootingMode(ShootingMode _mode);
	
	void SetConfidenceThreshold(float _threshold);
private:
	ControlMode m_controlMode;
	TrackingMode m_trackingMode;
	ShootingMode m_shootingMode;
	
	KinectStick *m_kinect;
	Joystick *m_leftStick, *m_rightStick;
	DriverStationEnhancedIO *m_enhancedIO;
	
	Team675::BeltSystem m_beltSystem;
	Team675::Turret m_turret;
	
	float m_thetaSetpoint;
	float m_phiSetpoint;
	float m_speedSetpoint;
	float m_confidenceThreshold;
	Target::TargetPosition m_targetSelection;
	
	void Shoot();
};
	
}

#endif
